/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
/**
 * @file: epuck_naive_coverage.cpp
 *
 * @date: February 08, 2013
 *
 * @author Jacopo De Stefani <jacopo.de.stefani@ulb.ac.be>
 * @author Manuele Brambilla <mbrambilla@iridia.ulb.ac.be>
 *
 */


#include "epuck_naive_coverage_controller.h"

using namespace argos;

static const CColor UNAVAILABLE_COLOR = CColor::YELLOW;
static const CColor AVAILABLE_COLOR = CColor::GREEN;
static const CColor BUSY_COLOR = CColor::RED;
static const CColor DISABLED_COLOR = CColor::ORANGE;

static const UInt32 ROTATION_STEPS = 8;

static const Real EPUCK_INTERWHEEL_DISTANCE = 0.053f; //5.3 cm -> 0.053m
static const Real EPUCK_KPROP = 50;
static const Real SATURATION_SPEED = 10.0;
static const Real CW_CCW_PROPORTION = 0.5;
static const Real IN_TAM_DISTANCE_THRESHOLD = 5.5;
static const Real ASSESSING_DISTANCE_THRESHOLD = 25.0f;
static const UInt16 TURNING_STEPS = 3;
static const UInt16 OBSTACLE_AVOIDANCE_STEPS = 15;
static const CRadians DIRECTION_CORRECTION = 3/2*CRadians::PI_OVER_FOUR;
static const Real PROXIMITY_SCALING_FACTOR = 15.0f;
static const Real OBSTACLE_AVOIDANCE_THRESHOLD = 15.0f; //15 cm
static const UInt32 BLOCKED_THRESHOLD = 200;

static const UInt32 PREY1 = 0;
static const UInt32 PREY2 = 1;

/****************************************/

/****************************************/

CEPuckNaiveCoverageController::CEPuckNaiveCoverageController() :
		cRandomRange(0, OBSTACLE_AVOIDANCE_STEPS),
		cRandomPac(0, 1),
		m_cRandomTurnThreshold(CRadians::PI-CRadians::PI_OVER_SIX*0.1f,CRadians::PI+CRadians::PI_OVER_SIX*0.1f){}

/****************************************/
/****************************************/

CEPuckNaiveCoverageController::~CEPuckNaiveCoverageController() {
}

/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::Init(TConfigurationNode& t_tree) {
    m_pcWheelsActuator = dynamic_cast<CCI_EPuckWheelsActuator*> (GetRobot().GetActuator("epuck_wheels"));
    m_pcLedActuator = dynamic_cast<CCI_EPuckLedsActuator*> (GetRobot().GetActuator("epuck_leds"));
    m_pcEncoderSensor = dynamic_cast<CCI_EPuckEncoderSensor*> (GetRobot().GetSensor("epuck_encoder"));
    m_pcProximitySensor = dynamic_cast<CCI_EPuckProximitySensor*> (GetRobot().GetSensor("epuck_proximity"));
    m_pcCameraSensor = dynamic_cast<CCI_EPuckOmnidirectionalCameraSensor*> (GetRobot().GetSensor("epuck_omnidirectional_camera"));
    m_pcCameraSensor->Enable();

    m_pcRNG = CARGoSRandom::CreateRNG("argos");

    m_enFMS = LINEAR_WALK;
    m_eRotationDirection = CLOCKWISE;

    TConfigurationNode tEpuckNode = GetNode(t_tree, "controller_parameters");
    GetNodeAttributeOrDefault(tEpuckNode, "wheel_speed", m_wheelSpeed, Real(5));
    GetNodeAttributeOrDefault(tEpuckNode, "proximity_threshold", m_fProximityThreshold, Real(0.2));
    GetNodeAttributeOrDefault(tEpuckNode, "camera_threshold", m_fCameraThreshold, Real(0.50));
    GetNodeAttributeOrDefault(tEpuckNode, "prox_weight", m_fProximityWeight, Real(0.8));

    GetNodeAttribute(tEpuckNode, "behavior", m_sBehaviorKind);

    tEpuckNode = GetNode(t_tree, "random_walk");
    GetNodeAttributeOrDefault(tEpuckNode, "change_direction_probability", m_fChangeDirectionProbability, Real(0.2));
    GetNodeAttributeOrDefault(tEpuckNode,"random_walk_turns", m_randomWalkTurns,  10);
    cRandomRange.Set(0, m_randomWalkTurns);

    m_cRandomTurnThreshold.Set(CRadians::PI-CRadians::PI_OVER_SIX*0.3f,CRadians::PI+CRadians::PI_OVER_SIX*0.3f);

    /* init desired direction */
    m_cDesiredDirection.SetFromAngleAndLength(CRadians::ZERO, 1.0);
    m_sLastVisitedClusterPosition.m_cPositionWRTRobot.SetFromAngleAndLength(CRadians::ZERO, 0.0);
    m_bIsSensingLateralWall = false;
    m_unTurningSteps = 0;

    tEpuckNode = GetNode(t_tree, "exploration");
    GetNodeAttributeOrDefault(tEpuckNode, "duration", m_fExplorationDuration, Real(1000));
    GetNodeAttributeOrDefault(tEpuckNode,"blind_period", m_fExplorationBlindPeriod,  Real(30));
    //	// Parameters for the sensor thresholds
    //	TConfigurationNode tThresholdNode = GetNode( t_tree, "thresholds");
    ////	LOG << "Threshold Proximity: " << m_fProximityThreshold<< std::endl;
    ////	LOG << "Threshold Ground: " << m_fGroundThreshold<< std::endl;
    //
    //	// Parameters for the probability function
    //	TConfigurationNode tProbabilityNode = GetNode( t_tree, "probability");
    //	GetNodeAttribute( tProbabilityNode, "Pac_min", m_fPac_min);
    //	GetNodeAttribute( tProbabilityNode, "Pac_exp", m_fPac_exp);
    //	GetNodeAttribute( tProbabilityNode, "stop_time", m_unStopStepsDefault);

    m_nRotationSteps = 0;
    m_nRandomTurnSteps = 0;
    m_nExplorationSteps = 0;
    m_nBlindSteps = 0;
    m_fLeftSpeed = 0;
    m_fRightSpeed = 0;
    m_fEncoderDistanceLeftWheel = 0;
    m_fEncoderDistanceRightWheel = 0;
    m_bIsOccpuationChanged = false;
    m_nBlockedSteps = 0;

    m_hasPrey1 = false;
    m_hasPrey2 = false;

    m_pcLedActuator->SwitchAllLeds(false);

    ChooseNextPrey();

}

/****************************************/

/****************************************/

void CEPuckNaiveCoverageController::ControlStep() {

	CVector2 combinedVector;

	ParseEncoder();
    ParseProximity();
    ParseCamera();
    m_prey1Trigger = false;
    m_prey2Trigger = false;

    //    LOG << GetRobot().GetRobotId() << ": state " << m_enFMS << std::endl;


    switch (m_enFMS) {
        case LINEAR_WALK:
        {
//        	if (m_nRotationSteps > 0) {
//        		m_pcWheelsActuator->SetLinearVelocity(m_wheelSpeed, -m_wheelSpeed);
//        		m_nRotationSteps--;
//        		break;
//        	}
        	m_pcWheelsActuator->SetLinearVelocity(m_wheelSpeed, m_wheelSpeed);

        	/*if(m_pcRNG->Uniform(CRange <Real>(0.0, 1.0)) < m_fChangeDirectionProbability) {
        	  	  CalculateRandomDirectionChange();
        	      m_enFMS = RANDOM_TURN;
        	      m_pcLedActuator->SwitchAllLeds(false);
        	      break;
        	      //m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        	      //LOG << GetRobot().GetRobotId() << " -> " << m_cDesiredDirection << std::endl;
			}*/

        	/** If an obstacle is sensed, perform a random turn */
        	if (m_obstacleSensed) {
        	   CalculateRandomDirectionChange();
        	   m_enFMS = RANDOM_TURN;
        	   m_pcLedActuator->SwitchAllLeds(false);
        	   //LOG << GetRobot().GetRobotId() << ": OBSTACLE" << std::endl;
        	   //LOG << GetRobot().GetRobotId() << ": " << m_nBlindSteps << std::endl;
        	   break;
        	}

        	/** If the robot isn't performing blind walk, direct to any seen blob */
        	if(m_nBlindSteps <= 0){
        		if(m_isBoothAvailable || m_isClosestBoothBusy || m_isClosestBoothUnavailable){
        			m_enFMS = DIRECTING_TO_TAM;
        			m_nBlockedSteps = BLOCKED_THRESHOLD;
        			m_pcLedActuator->SwitchAllLeds(false);
        			//LOG << GetRobot().GetRobotId() << ": DIRECTING_TO_TAM" << std::endl;
        			break;
        		}
        	}
        	else{ /** If the robot is performing blind walk, ignore information from the camera */
        		m_nBlindSteps--;
        		if(m_nBlindSteps <= 0){
        			//LOG << GetRobot().GetRobotId() << ": END_BLIND" << std::endl;
        		}
        	}
            break;
        }
        case RANDOM_TURN:
        {
        	if(m_nBlindSteps > 0){
        		m_nBlindSteps--;
        	}

        	if(!cRandomRange.WithinMinBoundIncludedMaxBoundIncluded(m_nRandomTurnSteps)) {
        		m_nRandomTurnSteps = cRandomRange.GetMax();
        	}

        	switch(m_eRotationDirection){
        		case CLOCKWISE:
        			m_pcWheelsActuator->SetLinearVelocity(-m_wheelSpeed*0.5, m_wheelSpeed*0.5);
        			break;
        		case COUNTERCLOCKWISE:
        			m_pcWheelsActuator->SetLinearVelocity(m_wheelSpeed*0.5, -m_wheelSpeed*0.5);
        			break;
        	}
        	if (--m_nRandomTurnSteps <= 0) {
        	    m_enFMS = LINEAR_WALK;
        	    m_pcLedActuator->SwitchAllLeds(false);
        	}
        	break;
        }
        case DIRECTING_TO_TAM:
        {
        	/** If the robot is going to an available booth,*/
        	if(!m_isBoothAvailable){
        		/** and the booth becomes unavailable, change state */
        		if(m_isClosestBoothBusy || m_isClosestBoothUnavailable){
        			/** If the robot is closer than EXPLORING_DISTANCE_THRESHOLD, it will start exploring the cluster*/
        			if(m_pcClosestOtherBoothBlob->Distance < ASSESSING_DISTANCE_THRESHOLD){
        				m_enFMS = ASSESSING_CLUSTER;
        				m_nExplorationSteps = 0;
        				//LOG << GetRobot().GetRobotId() << ": ASSESSING_CLUSTER" << std::endl;
        			}
        			else{ /** Otherwise direct to blob, taking into account proximity sensors information */
        				combinedVector.SetFromAngleAndLength(m_pcClosestOtherBoothBlob->Angle,1.0f);
        				combinedVector+= RescaleProximity();
        				ComputeSpeedFromAngle(combinedVector.Angle());
        				m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        			}
        			break;
        		}
        		else{
        		/** and no blob can be seen, perform a random turn */
        		CalculateRandomDirectionChange();
        		//LOG << GetRobot().GetRobotId() << ": LOST_TAM" << std::endl;
        		//m_enFMS = ODOMETRY_NAVIGATION;
        		m_enFMS = RANDOM_TURN;
        		m_pcLedActuator->SwitchAllLeds(false);
        		break;
        		}
        	}
        	else{
        		if(m_pcClosestAvailableBoothBlob->Distance < IN_TAM_DISTANCE_THRESHOLD){
        			m_enFMS = INTO_TAM;
        			m_pcLedActuator->SwitchAllLeds(false);
        			LOG << GetRobot().GetRobotId() << ": INTO_TAM" << std::endl;
        		}
        		else{
        			/** Perform rotation, if required to go to blob */
        			if (m_nRotationSteps > 0) {
        				m_pcWheelsActuator->SetLinearVelocity(m_wheelSpeed, -m_wheelSpeed);
        				m_nRotationSteps--;
        				break;
        			}
        			/** When closer to the booth, signal its presence to be avoided by the other robots */
        			if(m_pcClosestAvailableBoothBlob->Distance < OBSTACLE_AVOIDANCE_THRESHOLD){
        				m_pcLedActuator->SwitchAllLeds(true);
        			}

        			/** Direct to blob, taking into account proximity sensors information */
        			combinedVector.SetFromAngleAndLength(m_pcClosestAvailableBoothBlob->Angle,1.0f);
        			combinedVector+= RescaleProximity();
        			ComputeSpeedFromAngle(combinedVector.Angle());
        			m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        		}
        	}
           	break;
        }
        case ASSESSING_CLUSTER:
        {
        	/*if(m_bIsOccpuationChanged){
        	     TakeDecision();
        	}*/

        	/**If the request of the cluster currently being assessed is satisfied, leave the cluster*/
        	if(m_sClusterInformation.isValid){
        		if(m_sClusterInformation.clusterOccupation == m_sClusterInformation.clusterRequest){
        			//LOG << GetRobot().GetRobotId() << ": REQUEST_SATISFIED" << std::endl;
        			CalculateRandomDirectionChange();
        			m_nBlindSteps = m_fExplorationBlindPeriod;
        			m_enFMS = RANDOM_TURN;
        			m_pcLedActuator->SwitchAllLeds(false);
        			break;
        		}
        	}

        	/**If an available cluster is found, direct to it*/
        	if(m_isBoothAvailable){
        		m_enFMS = DIRECTING_TO_TAM;
        		//LOG << GetRobot().GetRobotId() << ": DIRECTING_TO_BOOTH" << std::endl;
        		m_pcLedActuator->SwitchAllLeds(false);
        		break;
        	}

        	/**Avoid unavabilable or busy cluster by directing in a different direction*/
        	if(m_isClosestBoothBusy || m_isClosestBoothUnavailable){
        		//LOG << GetRobot().GetRobotId() << "U:" << m_pcClosestUnavailableTAMBlob->Distance << std::endl;
        		//LOG << GetRobot().GetRobotId() << "B:" << m_pcClosestBusyTAMBlob->Distance << std::endl;
        		combinedVector.SetFromAngleAndLength(m_pcClosestOtherBoothBlob->Angle,2.0f);
        		combinedVector.Rotate(-1.5f*CRadians::PI_OVER_FOUR);
        		combinedVector+= RescaleProximity();
        		ComputeSpeedFromAngle(combinedVector.Angle());
        		m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);

        	}
        	else{/**If no blob to avoid is seen, follow a slightly bended trajectory*/
        		//LOG << GetRobot().GetRobotId() << ": EXPLORING_NO_BLOB" << std::endl;
        		combinedVector.SetFromAngleAndLength(CRadians::PI_OVER_SIX*0.1f,1.0f);
        		combinedVector+= RescaleProximity();
        		ComputeSpeedFromAngle(combinedVector.Angle());
        		m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        		/*m_enFMS = EXPLORING_CLUSTER_WALL;
        		m_pcLedActuator->SwitchAllLeds(false);
        		LOG << GetRobot().GetRobotId() << ": EXPLORING_TAM_WALL" << std::endl;*/
        	}

        	m_nExplorationSteps++;
        	break;
        }

        case INTO_TAM:
        {
        	/**Stop the robot when he is in the booth*/
        	m_pcWheelsActuator->SetLinearVelocity(0.0, 0.0);
        	/**Work if the booth requires it*/
        	if(m_isClosestBoothBusy){
        		m_enFMS = WORK;
        		m_pcLedActuator->SwitchAllLeds(true);
        		//LOG << GetRobot().GetRobotId() << ": WORK" << std::endl;
        	}
        	/**Leave it otherwise */
        	//else if(m_isClosestBoothUnavailable){
        		//TakeDecision();
        	//}
        	else if(m_isClosestBoothDisabled){
        		m_enFMS = EXIT_TAM;
        		m_nRotationSteps = ROTATION_STEPS;
        		m_pcLedActuator->SwitchAllLeds(false);
        		//LOG << GetRobot().GetRobotId() << ": EXIT_TAM" << std::endl;
        	}
        	break;
        }
        case EXIT_TAM:
        {
        	/**Perform half rotation*/
        	if (m_nRotationSteps > 0) {
        		m_pcWheelsActuator->SetLinearVelocity(m_wheelSpeed, -m_wheelSpeed);
        		m_nRotationSteps--;
        		break;
        	}
        	/**Then move straight to exit, performing obstacle avoidance*/
        	if(m_isClosestBoothUnavailable || m_isClosestBoothDisabled){
        		if(m_pcClosestOtherBoothBlob->Distance < OBSTACLE_AVOIDANCE_THRESHOLD){
        			m_pcLedActuator->SwitchAllLeds(true);
        		}
        		/**If the booth does not become available anymore, and the robot has surpassed ASSESSING DISTANCE THRESHOLD, then it starts again the random exploration*/
        		if(m_pcClosestOtherBoothBlob->Distance > ASSESSING_DISTANCE_THRESHOLD){
        			m_nBlindSteps = m_fExplorationBlindPeriod;
        			CalculateRandomDirectionChange();
        			m_enFMS = RANDOM_TURN;
        			m_pcLedActuator->SwitchAllLeds(false);
        			break;
        		}
        		else{
        			combinedVector.SetFromAngleAndLength(m_pcClosestOtherBoothBlob->Angle,1.0f);
        			combinedVector.Rotate(CRadians::PI);
        			combinedVector+= RescaleProximity();
        			ComputeSpeedFromAngle(combinedVector.Angle());
        			m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        			break;
        		}
        		m_pcLedActuator->SwitchAllLeds(true);
        		ComputeSpeedFromAngle(combinedVector.Angle());
        		m_pcWheelsActuator->SetLinearVelocity(m_fLeftSpeed, m_fRightSpeed);
        		break;
        	}
//        	/**If the booth is available, decide whether to enter it or not*/
//        	if(m_isBoothAvailable && m_pcClosestAvailableBoothBlob->Distance > ASSESSING_DISTANCE_THRESHOLD){
//        		TakeDecision();
//        		break;
//        	}
          	break;
        }
        case WORK:
        {
        	/**The robot is stopped and observing the booth state*/
        	m_pcWheelsActuator->SetLinearVelocity(0.0, 0.0);
        	/*if (m_isClosestBoothUnavailable || m_isClosestBoothDisabled) {
        		m_enFMS = INTO_TAM;
        		m_nRotationSteps = ROTATION_STEPS;
        		m_pcLedActuator->SwitchBodyLed(false);
			}*/
        	break;
        }

    }

    //LOG << "[" << GetRobot().GetRobotId() << "] Left wheel: " << m_fEncoderDistanceLeftWheel << " - Right wheel:" << m_fEncoderDistanceRightWheel << std::endl;

    //    if (m_hasPrey){
    //        m_pcLedActuator->SwitchAllLeds(true);
    //    }
    //    else{
    //        m_pcLedActuator->SwitchAllLeds(false);
    //    }

   /*if(m_sClusterInformation.isValid){
	  LOG << "[" << GetRobot().GetRobotId() << "] : " << m_sClusterInformation.clusterOccupation << "/" << m_sClusterInformation.clusterRequest << std::endl;
   }*/
   m_sClusterInformation.isValid= false;
   m_bIsOccpuationChanged = false;


}



/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::ParseProximity() {
	CCI_EPuckProximitySensor::TReadings tReadings = m_pcProximitySensor->GetReadings();
    std::vector<CCI_EPuckProximitySensor::SReading> frontReadings;

    // consider only the front sensors
    //frontReadings.push_back(tReadings[2]);
    frontReadings.push_back(tReadings[1]);
    frontReadings.push_back(tReadings[0]);
    frontReadings.push_back(tReadings[7]);
    frontReadings.push_back(tReadings[6]);
    //frontReadings.push_back(tReadings[5]);

    Real maxProximity = frontReadings[0].Value;
    CRadians maxProximityAngle = frontReadings[0].Angle;
    for (UInt16 i = 0; i < frontReadings.size(); i++) {
        if (frontReadings[i].Value > maxProximity) {
            maxProximity = frontReadings[i].Value;
            maxProximityAngle = frontReadings[i].Angle;
        }
    }
    //LOG << GetRobot().GetRobotId() << "[Proximity] - Value: " << maxProximity << "- Angle:" << maxProximityAngle << std::endl;


   /*	if(m_enFMS == EXPLORING_CLUSTER_WALL){
   		if(tReadings[2].Value > 0){
   			m_bIsSensingLateralWall = true;
   			//LOG << GetRobot().GetRobotId() << ": LATERAL_WALL" << std::endl;
   			m_bChangeDirection = false;
   		}
   		else{
   			if(m_bIsSensingLateralWall){
   				m_bChangeDirection = true;
   				m_unTurningSteps = TURNING_STEPS;
   				LOG << GetRobot().GetRobotId() << ": CHANGE_DIRECTION" << std::endl;
   			}
   			m_bIsSensingLateralWall = false;
   			//LOG << GetRobot().GetRobotId() << ": NOT_LATERAL_WALL" << std::endl;
   		}
   		//LOG << GetRobot().GetRobotId() << "[Proximity Lateral] - Value: " << tReadings[2].Value << std::endl;
    }*/

    if (maxProximity >= m_fProximityThreshold) {
    	m_obstacleSensed = true;
    	//LOG << GetRobot().GetRobotId() << ": " << maxProximity << std::endl;
    	m_closestObstacleReversed.SetFromAngleAndLength(maxProximityAngle, maxProximity);
        m_closestObstacleReversed.Rotate(CRadians::PI);
    } else {
        m_obstacleSensed = false;
        m_closestObstacleReversed.SetFromAngleAndLength(CRadians::ZERO, 0.0f);
    }


}


/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::ParseCamera() {

    m_isBoothAvailable = false;
    m_pcClosestAvailableBoothBlob = NULL;
    m_pcClosestOtherBoothBlob = NULL;
    m_isClosestBoothBusy = false;
    m_isClosestBoothUnavailable = false;
    m_isClosestBoothDisabled = false;

    m_sCameraReadings = &m_pcCameraSensor->GetCameraReadings();

    std::vector<CCI_CameraSensor::SBlob*> allBlobs = m_sCameraReadings->BlobList;
    std::vector<CCI_CameraSensor::SBlob*> availableBlobs;
    std::vector<CCI_CameraSensor::SBlob*> unavailableBlobs;
    std::vector<CCI_CameraSensor::SBlob*> busyBlobs;
    std::vector<CCI_CameraSensor::SBlob*> disabledBlobs;

     // filter out blobs that are too distant and categorize them
    for (std::vector<CCI_CameraSensor::SBlob*>::iterator it = allBlobs.begin(); it != allBlobs.end(); ++it) {
        if ((*it)->Distance <= m_fCameraThreshold * 100) {
            if ((*it)->Color == AVAILABLE_COLOR) {
                availableBlobs.push_back(*it);
            } else if ((*it)->Color == UNAVAILABLE_COLOR) {
                unavailableBlobs.push_back(*it);
            } else if ((*it)->Color == BUSY_COLOR) {
                busyBlobs.push_back(*it);
            } else if ((*it)->Color == DISABLED_COLOR) {
            	disabledBlobs.push_back(*it);
            }
        }
    }

    // process available workspaces information
    if (availableBlobs.size() > 0) {
        m_isBoothAvailable = true;
        std::sort(availableBlobs.begin(), availableBlobs.end(), myBlobsSort);
        m_pcClosestAvailableBoothBlob = availableBlobs.at(0);
        m_sLastVisitedClusterPosition.m_cPositionWRTRobot.SetFromAngleAndLength(m_pcClosestAvailableBoothBlob->Angle,m_pcClosestAvailableBoothBlob->Distance*0.01f);
        //m_sLastVisitedClusterPosition.m_cAngularDisplacementWRTRobot = -m_closestTAMBlob->Angle;
        //LOG << GetRobot().GetRobotId() << " - Closest TAM Angle: " << m_closestTAMBlob->Angle << std::endl;

     }
    else{
    	//LOG << GetRobot().GetRobotId() << " - UPDATE_ODOMETRY: " << std::endl;
    	m_sLastVisitedClusterPosition.UpdatePosition(m_fEncoderDistanceLeftWheel,m_fEncoderDistanceRightWheel);
    }

    // process unavailable workspaces information
    if (unavailableBlobs.size() > 0) {
        std::sort(unavailableBlobs.begin(), unavailableBlobs.end(), myBlobsSort);
        m_pcClosestOtherBoothBlob = unavailableBlobs.at(0);
    }

    // process busy workspaces information
    if (busyBlobs.size() > 0) {
        std::sort(busyBlobs.begin(), busyBlobs.end(), myBlobsSort);
        if(m_pcClosestOtherBoothBlob != NULL){
        	if(busyBlobs.at(0)->Distance < m_pcClosestOtherBoothBlob->Distance){
        		m_pcClosestOtherBoothBlob = busyBlobs.at(0);
        	}
        }
        else{
        	m_pcClosestOtherBoothBlob = busyBlobs.at(0);
        }

    }

    // process unavailable workspaces information
    if (disabledBlobs.size() > 0) {
    	std::sort(disabledBlobs.begin(), disabledBlobs.end(), myBlobsSort);
    	m_pcClosestOtherBoothBlob = disabledBlobs.at(0);
    }

    if(m_pcClosestOtherBoothBlob != NULL){
    	if(m_pcClosestOtherBoothBlob->Color == UNAVAILABLE_COLOR){
    		m_isClosestBoothUnavailable = true;

    	} else if(m_pcClosestOtherBoothBlob->Color == BUSY_COLOR){
    			m_isClosestBoothBusy = true;
    	} else if(m_pcClosestOtherBoothBlob->Color == DISABLED_COLOR){
			m_isClosestBoothDisabled = true;
    	}
	}
}

/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::ParseEncoder() {
	CCI_EPuckEncoderSensor::SReading sReading = m_pcEncoderSensor->GetReading();
	/**Distance conversion from cm to meters*/
	m_fEncoderDistanceLeftWheel = sReading.CoveredDistanceLeftWheel*0.01f;
	m_fEncoderDistanceRightWheel = sReading.CoveredDistanceRightWheel*0.01f;
}


/****************************************/
/****************************************/

bool CEPuckNaiveCoverageController::myBlobsSort(CCI_CameraSensor::SBlob* blob1, CCI_CameraSensor::SBlob* blob2) {
    return (blob1->Distance < blob2->Distance);
}


/****************************************/
/****************************************/
void CEPuckNaiveCoverageController::ComputeSpeedFromAngle(CRadians c_target_angle) {
    Real fCurrentForwardVelocity = m_wheelSpeed;
    Real fDotProduct = 0.0;


    if (c_target_angle > CRadians::PI_OVER_TWO || c_target_angle < -CRadians::PI_OVER_TWO) {
        fDotProduct = 0.0;
    } else {
        CVector2 cForwardVector(1.0, CRadians::ZERO);
        CVector2 cTargetVector(1.0, c_target_angle);
        fDotProduct = cForwardVector.DotProduct(cTargetVector);
    }

    CRadians fAngularVelocity = EPUCK_KPROP * c_target_angle;
    m_fLeftSpeed = fDotProduct * fCurrentForwardVelocity - fAngularVelocity.GetValue() * EPUCK_INTERWHEEL_DISTANCE;
    m_fRightSpeed = fDotProduct * fCurrentForwardVelocity + fAngularVelocity.GetValue() * EPUCK_INTERWHEEL_DISTANCE;

    //    LOG << m_fLeftSpeed << " - " << m_fRightSpeed << std::endl;

}


/****************************************/
/****************************************/

CVector2 CEPuckNaiveCoverageController::RescaleProximity() {

    CVector2 proximity_weighted;

    switch(m_enFMS){
    case LINEAR_WALK:
    case ASSESSING_CLUSTER:
    	proximity_weighted.SetFromAngleAndLength(m_closestObstacleReversed.Angle(),PROXIMITY_SCALING_FACTOR*m_closestObstacleReversed.Length());
    	break;
    case DIRECTING_TO_TAM:
    	if(m_isBoothAvailable){
    		if(m_pcClosestAvailableBoothBlob->Distance < OBSTACLE_AVOIDANCE_THRESHOLD){
    			proximity_weighted.SetFromAngleAndLength(m_closestObstacleReversed.Angle(),0.8f);
    		}
    		else{
    			proximity_weighted.SetFromAngleAndLength(m_closestObstacleReversed.Angle(),PROXIMITY_SCALING_FACTOR*m_closestObstacleReversed.Length());
    		}
    	}
    	else{
    		if(m_isClosestBoothBusy || m_isClosestBoothUnavailable){
    			proximity_weighted.SetFromAngleAndLength(m_closestObstacleReversed.Angle(),PROXIMITY_SCALING_FACTOR*m_closestObstacleReversed.Length());
    		}
    	}
    	break;
    case INTO_TAM:
    case WORK:
    case EXIT_TAM:
    	proximity_weighted.SetFromAngleAndLength(m_closestObstacleReversed.Angle(),0.8f);
    	break;
    case RANDOM_TURN:
    	proximity_weighted = m_closestObstacleReversed;
    	break;
    }

    return proximity_weighted;
}

/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::ChooseNextPrey(){

    if (m_sBehaviorKind == "random"){
        if (m_pcRNG->Bernoulli(0.5)){
            m_targetPrey = PREY1;
        }
        else{
            m_targetPrey = PREY2;
        }
    }
    else if (m_sBehaviorKind == "only1") {
        m_targetPrey = PREY1;
    }
    else if (m_sBehaviorKind == "best") {
        //todo
    }
    else {
        THROW_ARGOSEXCEPTION("Behavior '" << m_sBehaviorKind << "' does not exist! Fix the XML.");
    }

    //LOG << GetRobot().GetRobotId() << ": next target is " << m_targetPrey << std::endl;

}

/****************************************/

/****************************************/

CEPuckNaiveCoverageController::EnFSM CEPuckNaiveCoverageController::GetState() {
    return m_enFMS;
}


/****************************************/

/****************************************/

void CEPuckNaiveCoverageController::SetClusterInformation(UInt16 un_cluster_id,UInt16 un_cluster_request,UInt16 un_cluster_occupation){
	m_bIsOccpuationChanged = (m_sClusterInformation.clusterOccupation != un_cluster_occupation);
	m_sClusterInformation.clusterId = un_cluster_id;
	m_sClusterInformation.clusterRequest = un_cluster_request;
	m_sClusterInformation.clusterOccupation = un_cluster_occupation;
	m_sClusterInformation.isValid = true;
}

void CEPuckNaiveCoverageController::Destroy() {

}


/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::CalculateRandomDirectionChange()
{
	m_nRandomTurnSteps = m_pcRNG->Uniform(cRandomRange);
	if(m_pcRNG->Uniform(CRange <Real>(0.0, 1.0)) < CW_CCW_PROPORTION){
		m_eRotationDirection = CLOCKWISE;
	}
	else{
		m_eRotationDirection = COUNTERCLOCKWISE;
	}
}

/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::SetDesiredDirection(CVector2& c_direction)
{
	m_cDesiredDirection = c_direction;

}

/****************************************/
/****************************************/

CVector2 CEPuckNaiveCoverageController::GetDesiredDirection() const
{
return m_cDesiredDirection;
}

/****************************************/
/****************************************/

Real CEPuckNaiveCoverageController::SaturateLinearSpeed()
{
	Real fResult = m_fLinearSpeed;
	if(m_fLinearSpeed > SATURATION_SPEED) {
		fResult = SATURATION_SPEED;
	}
	if(m_fLinearSpeed < -SATURATION_SPEED) {
		fResult = -SATURATION_SPEED;
	}
	return fResult;
}


/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::SetChangeDirectionProbability(Real f_change_direction_probability)
{
m_fChangeDirectionProbability = f_change_direction_probability;
}

/****************************************/
/****************************************/

Real CEPuckNaiveCoverageController::GetChangeDirectionProbaility() const
{
return m_fChangeDirectionProbability;
}

/****************************************/
/****************************************/

bool CEPuckNaiveCoverageController::IsTamAvailable() const {
	return m_isBoothAvailable;
}

/****************************************/
/****************************************/

const CEPuckNaiveCoverageController::SClusterExpectedPosition& CEPuckNaiveCoverageController::GetClosestTamExpectedPosition() const {
	return m_sLastVisitedClusterPosition;
}

/***************************************
**************************************

CRadians CEPuckNaiveCoverageController::SClusterExpectedPosition::GetAngle() {
	return -(CRadians::PI_OVER_TWO + ATan2(position.GetX(), position.GetY()));
}

**************************************
**************************************

Real  CEPuckNaiveCoverageController::SClusterExpectedPosition::GetDistance()  {
    return sqrt(position.GetX()*position.GetX()+position.GetY()*position.GetY());
}*/


/****************************************/
/****************************************/

void CEPuckNaiveCoverageController::SClusterExpectedPosition::UpdatePosition(Real covered_distance_left_wheel, Real covered_distance_right_wheel) {
	//distL = distance made by left wheel. distR for Right wheel. InterWheel is the distance between wheels.
	//All the distances are in meters.

	Real stepLinearDisplacement = (covered_distance_left_wheel+covered_distance_right_wheel)/2;
	CRadians stepAngularDisplacement = -CRadians((covered_distance_left_wheel-covered_distance_right_wheel)/(EPUCK_INTERWHEEL_DISTANCE));

	//LOG << "Linear:" << stepLinearDisplacement << "- Angular: " << stepAngularDisplacement << std::endl;

	//confidenceLevel += (Abs(distR)+Abs(distL))/100;
	//m_cPositionWRTRobot += CVector2( stepDist, stepAngle);
	//m_cPositionWRTRobot.Set(m_cPositionWRTRobot.GetX()-Cos(m_cAngularDisplacementWRTRobot)*stepLinearDisplacement,m_cPositionWRTRobot.GetY()-Sin(m_cAngularDisplacementWRTRobot)*stepLinearDisplacement);
	m_cPositionWRTRobot -= CVector2(stepLinearDisplacement,stepAngularDisplacement);
	m_cPositionWRTRobot.Rotate(-stepAngularDisplacement);

}

REGISTER_CONTROLLER(CEPuckNaiveCoverageController, "epuck_naive_coverage_controller");
